{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "70602c51",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Jupyter environment detected. Enabling Open3D WebVisualizer.\n",
      "[Open3D INFO] WebRTC GUI backend enabled.\n",
      "[Open3D INFO] WebRTCWindowSystem: HTTP handshake server disabled.\n"
     ]
    }
   ],
   "source": [
    "import open3d as o3d\n",
    "import numpy as np\n",
    "import copy\n",
    "import time\n",
    "import os\n",
    "import sys"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8f4176bb",
   "metadata": {},
   "source": [
    "## 可视化"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "55ea16d8",
   "metadata": {},
   "outputs": [],
   "source": [
    "def draw_registration_result(source, target, transformation):\n",
    "    source_temp = copy.deepcopy(source)\n",
    "    target_temp = copy.deepcopy(target)\n",
    "    source_temp.paint_uniform_color([1, 0.706, 0])\n",
    "    target_temp.paint_uniform_color([0, 0.651, 0.929])\n",
    "    source_temp.transform(transformation)\n",
    "    o3d.visualization.draw_geometries([source_temp, target_temp])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "9ec83234",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      ":: Load two point clouds and disturb initial pose.\n"
     ]
    }
   ],
   "source": [
    "# demo_icp_pcds = o3d.data.DemoICPPointClouds()\n",
    "# source = o3d.io.read_point_cloud(demo_icp_pcds.paths[0])\n",
    "# target = o3d.io.read_point_cloud(demo_icp_pcds.paths[1])\n",
    "\n",
    "s='../VIC/infra/cooperative-vehicle-infrastructure-infrastructure-side-velodyne/004375.pcd'\n",
    "t='../VIC/vehic/cooperative-vehicle-infrastructure-vehicle-side-velodyne/004375.pcd'\n",
    "print(\":: Load two point clouds and disturb initial pose.\")\n",
    "source = o3d.io.read_point_cloud(s)\n",
    "target = o3d.io.read_point_cloud(t)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "b956133f",
   "metadata": {},
   "outputs": [],
   "source": [
    "threshold = 0.12\n",
    "trans_init = np.asarray([[0.462, 0.011, -0.507, 0.5],\n",
    "                         [-0.139, 0.967, -0.215, 0.7],\n",
    "                         [0.487, 0.255, 0.835, 5.4], \n",
    "                         [0.0, 0.0, 0.0, 1.0]])\n",
    "draw_registration_result(source, target, trans_init)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9264fd6c",
   "metadata": {},
   "source": [
    "## 评估"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "388ad3f0",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Initial alignment\n",
      "RegistrationResult with fitness=0.000000e+00, inlier_rmse=0.000000e+00, and correspondence_set size of 0\n",
      "Access transformation to get result.\n"
     ]
    }
   ],
   "source": [
    "print(\"Initial alignment\")\n",
    "evaluation = o3d.pipelines.registration.evaluate_registration(\n",
    "    source, target, threshold, trans_init)\n",
    "print(evaluation)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2887683f",
   "metadata": {},
   "source": [
    "## ICP"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "dcad44f5",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "id": "ee273be3",
   "metadata": {},
   "outputs": [
    {
     "ename": "TypeError",
     "evalue": "registration_generalized_icp(): incompatible function arguments. The following argument types are supported:\n    1. (source: open3d.cpu.pybind.geometry.PointCloud, target: open3d.cpu.pybind.geometry.PointCloud, max_correspondence_distance: float, init: numpy.ndarray[numpy.float64[4, 4]] = array([[1., 0., 0., 0.],\n       [0., 1., 0., 0.],\n       [0., 0., 1., 0.],\n       [0., 0., 0., 1.]]), estimation_method: open3d.cpu.pybind.pipelines.registration.TransformationEstimationForGeneralizedICP = TransformationEstimationForGeneralizedICP with epsilon=0.001000, criteria: open3d.cpu.pybind.pipelines.registration.ICPConvergenceCriteria = ICPConvergenceCriteria class with relative_fitness=1.000000e-06, relative_rmse=1.000000e-06, and max_iteration=30) -> open3d.cpu.pybind.pipelines.registration.RegistrationResult\n\nInvoked with: PointCloud with 64508 points., PointCloud with 61220 points., 0.12, array([[ 0.462,  0.011, -0.507,  0.5  ],\n       [-0.139,  0.967, -0.215,  0.7  ],\n       [ 0.487,  0.255,  0.835,  5.4  ],\n       [ 0.   ,  0.   ,  0.   ,  1.   ]]), TransformationEstimationPointToPoint without scaling., ICPConvergenceCriteria class with relative_fitness=1.000000e-06, relative_rmse=1.000000e-06, and max_iteration=20000",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mTypeError\u001b[0m                                 Traceback (most recent call last)",
      "\u001b[0;32m/tmp/ipykernel_11372/3761036598.py\u001b[0m in \u001b[0;36m<module>\u001b[0;34m\u001b[0m\n\u001b[1;32m      7\u001b[0m     \u001b[0msource\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mtarget\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mthreshold\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mtrans_init\u001b[0m\u001b[0;34m,\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m      8\u001b[0m     \u001b[0mo3d\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mpipelines\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mregistration\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mTransformationEstimationPointToPoint\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m,\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m----> 9\u001b[0;31m     o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=20000))\n\u001b[0m\u001b[1;32m     10\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     11\u001b[0m \u001b[0mprint\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mreg_p2p\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;31mTypeError\u001b[0m: registration_generalized_icp(): incompatible function arguments. The following argument types are supported:\n    1. (source: open3d.cpu.pybind.geometry.PointCloud, target: open3d.cpu.pybind.geometry.PointCloud, max_correspondence_distance: float, init: numpy.ndarray[numpy.float64[4, 4]] = array([[1., 0., 0., 0.],\n       [0., 1., 0., 0.],\n       [0., 0., 1., 0.],\n       [0., 0., 0., 1.]]), estimation_method: open3d.cpu.pybind.pipelines.registration.TransformationEstimationForGeneralizedICP = TransformationEstimationForGeneralizedICP with epsilon=0.001000, criteria: open3d.cpu.pybind.pipelines.registration.ICPConvergenceCriteria = ICPConvergenceCriteria class with relative_fitness=1.000000e-06, relative_rmse=1.000000e-06, and max_iteration=30) -> open3d.cpu.pybind.pipelines.registration.RegistrationResult\n\nInvoked with: PointCloud with 64508 points., PointCloud with 61220 points., 0.12, array([[ 0.462,  0.011, -0.507,  0.5  ],\n       [-0.139,  0.967, -0.215,  0.7  ],\n       [ 0.487,  0.255,  0.835,  5.4  ],\n       [ 0.   ,  0.   ,  0.   ,  1.   ]]), TransformationEstimationPointToPoint without scaling., ICPConvergenceCriteria class with relative_fitness=1.000000e-06, relative_rmse=1.000000e-06, and max_iteration=20000"
     ]
    }
   ],
   "source": [
    "# reg_p2p = o3d.pipelines.registration.registration_icp(\n",
    "#     source, target, threshold, trans_init,\n",
    "#     o3d.pipelines.registration.TransformationEstimationPointToPoint(),\n",
    "#     o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=20000))\n",
    "\n",
    "reg_p2p=o3d.pipelines.registration.registration_generalized_icp(\n",
    "    source, target, threshold, trans_init,\n",
    "    o3d.pipelines.registration.TransformationEstimationPointToPoint(),\n",
    "    o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=20000))\n",
    "\n",
    "print(reg_p2p)\n",
    "print(\"Transformation is:\")\n",
    "print(reg_p2p.transformation)\n",
    "draw_registration_result(source, target, reg_p2p.transformation)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "id": "35ff9215",
   "metadata": {},
   "outputs": [],
   "source": [
    "target.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.08, max_nn=30))\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "id": "4b7931c0",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Apply point-to-plane ICP\n",
      "RegistrationResult with fitness=0.000000e+00, inlier_rmse=0.000000e+00, and correspondence_set size of 0\n",
      "Access transformation to get result.\n",
      "Transformation is:\n",
      "[[ 0.462  0.011 -0.507  0.5  ]\n",
      " [-0.139  0.967 -0.215  0.7  ]\n",
      " [ 0.487  0.255  0.835  5.4  ]\n",
      " [ 0.     0.     0.     1.   ]]\n"
     ]
    }
   ],
   "source": [
    "print(\"Apply point-to-plane ICP\")\n",
    "reg_p2l = o3d.pipelines.registration.registration_icp(\n",
    "    source, target, threshold, trans_init,\n",
    "    o3d.pipelines.registration.TransformationEstimationPointToPlane())\n",
    "print(reg_p2l)\n",
    "print(\"Transformation is:\")\n",
    "print(reg_p2l.transformation)\n",
    "draw_registration_result(source, target, reg_p2l.transformation)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "06bbdf77",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.16"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
